
/**********************************************************************\
*																																	  	 *
* PROGRAM: Omni-Bot driver     	      															  *
* VERSION: 11 																										    *
* PURPOSE: This program is used to control an omni directional robot  *
*          that drives relative to the user                           *
* AUTHOR: Samuel Rosenstein                                           *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/


#pragma systemFile //stops "unreferenced "bla" for coming up
//This is the source code for driving the omni-robot driving
//The omni-robot's motors should be set up like the following:
/*
front       joystick
b/------\c      | /     hence...the robot is 45 degrees off from the joystick
 |      |    ___|/____
 |      |       |
a\------/d      |

\  /  <- graph for robot (aproximitly)
 \/
 /\
/  \



*/
#ifndef OMNIDRIVER
#define OMNIDRIVER
#endif
int Jdrivea = 0;
int Jdriveb = 0;
int Jdrivec = 0;
int Jdrived = 0;


/*
//These global variables are used for gyro sensor
int rotationSpeed = 0;     //The current rotation speed, which could include drifts (in deg/sec)
int rotationSpeedAv = 0;  //The average of the current rotation speed (in deg/sec)
int readings;        //amount of readings that are being taken (4)
float degree = 0;   //float of the degrees (best to 2 decimal points)
int degrees;        //this is the degree in integer value with direction from calibration point (negative if left)
long outputDeg;     //This is the final output in degrees

task fgyro {
//Assuming that the Gyro has already been initiallized

degree = 0;
degrees = 0;
outputDeg = 0;
while(true) {
//start readings (read 4, then take the average of the 4 readings)

while(readings < 4) {
rotationSpeed = HTGYROreadRot(gyro) + rotationSpeed;
wait1Msec(1);
readings = readings + 1; }
//end readings
//start converting
rotationSpeedAv = ((float)rotationSpeed) / (readings) ;
if (rotationSpeed > 8 || rotationSpeedAv < -8) {        //This if statement takes care of the gyro drift
degree = degree + (rotationSpeedAv * 0.005);}           //0.005 seconds is how long it takes to take 4 samples
if (degree > 359){
degree = degree - 360;}
if (degree < -359){
degree = degree + 360;}
degrees = degree;
outputDeg = degrees;
if (outputDeg < 0){
outputDeg = outputDeg + 360;}

readings = 0;
rotationSpeed = 0;
//end converting
//reset variables
}
}
*/

task Jdrive{
  long vector;        //angle for the vector of the robot (from the x axis)
  int theda;          //internal angle of "joy triangle"
  long joyH;          //hypotinuse of "joy triangle"
  int robotx;         //kind of obvious....
  int roboty;         // same with this one...
  int returning;      //this is what the function returns
  int newTheda;       //this is once all of the calculations are done
  int joyx;
  int joyy;
  int spin[4];

  //intitializing done
  //Start of readings for "omni-spin"
  while(true){
    getJoystickSettings(joystick);
    if((joystick.joy1_x1/1.8) > 12 || (joystick.joy1_x1/1.8) < -12) {
      spin[0] = joystick.joy1_x1/3;
      spin[1] = joystick.joy1_x1/3;
      spin[2] = -1 * joystick.joy1_x1/3;
      spin[3] = -1 * joystick.joy1_x1/3; }
    else {
      spin[0] = 0;
      spin[1] = 0;
      spin[2] = 0;
      spin[3] = 0;
    }

    //end of readings for "omin-spin" and they are stored in the array Spin, which each spot corresponding to a motor
    //Start of joystick readings for driving
    getJoystickSettings(joystick);

    joyx = joystick.joy1_x2/1.27;
    joyy = joystick.joy1_y2/1.27;
    joyH = sqrt((joyy*joyy) + (joyx*joyx));
    if (joyy == 0 && joyx == 0) { theda = 0; }
    else { theda = radiansToDegrees(asin(((float)abs(joyy))/abs(joyH))); }

    //equations done
    //start of setting up vectors

    if (joyy > 0 && joyx >= 0) {          //1st quadrant
        vector = theda; }
    else if (joyy > 0 && joyx <= 0) {     //2nd quadrant
        vector = 180 - theda; }
    else if (joyy < 0 && joyx <= 0) {     //3rd quadrant
        vector = 180 + theda; }
    else if (joyy < 0 && joyx >= 0) {     //4th quadrant
        vector = 360 - theda; }
    else if (joyy == 0 && joyx > 0) { vector = 0; }     //b/c of division, if joyy = 0 then theda = 0
    else if (joyy == 0 && joyx < 0) { vector = 180; }  // same as above (this time though, the line is on the 180 degree mark)

    //then check to make sure that the vector isn't on the ORIGIN, and if it isn't, add 45 degrees

    if (joyy == 0 && joyx == 0) { vector = 0; }
    else {vector = vector + 45 + heading;}               // +45 b/c the wheels are 45 degrees off of the original
      if (vector > 359) {                                    // joystick graph (check notebook for more info)
        vector = vector - 360;
        if (vector > 359) {                                    // run twice incase the value is greater than 720
          vector = vector - 360; }
      }
    //end of setting up vectors (with the +45 b/c of wheel allignment)
    //start setting up motors ROBOTX IS REVERSED BECAUSE OF REVERSED MOTORS
    //SO...POINTS FOR X ARE "-" INSTEAD OF POSTITIVE

    if (vector < 90 && vector > 0) {                        //1st quadrant
        newTheda = vector;
      robotx =  -1 * joyH * cosDegrees(newTheda);
      roboty = joyH * sinDegrees(newTheda);
    }
    else if (vector > 90 && vector < 180) {  //2nd quadrant
        newTheda = 180 - vector;
      robotx = joyH * cosDegrees(newTheda);
      roboty = joyH * sinDegrees(newTheda);
    }
    else if (vector > 180 && vector < 270) { //3rd quadrant
        newTheda = 90 - (270 - vector);
      robotx = joyH * cosDegrees(newTheda);
      roboty = -1 * joyH * sinDegrees(newTheda);
    }
    else if (vector > 270) {                 //4th quadrant
        newTheda = 360 - vector;
      robotx = -1 * joyH * cosDegrees(newTheda);
      roboty = -1 * joyH * sinDegrees(newTheda);
    }
    else if (vector == 0) {       //if the vector is on a quadrant line (Ex: 90 degrees)
        robotx = -1 * joyH;
      roboty = 0; }
    else if (vector == 90) {
      robotx = 0;
      roboty = joyH; }
    else if (vector == 180) {
      robotx = joyH;
      roboty = 0; }
    else if (vector == 270) {
      robotx = 0;
      roboty = -1 * joyH; }

    //end of setting up robotx and roboty
    //then send out the correct variables for the motors

    Jdrivea = robotx + spin[0];
    Jdriveb = roboty + spin[1];
    Jdrivec = robotx + spin[2];
    Jdrived = roboty + spin[3];
  }
  wait1Msec(40);
  //End of EVERYTHING
}
